/*
 * Javlov - a Java toolkit for reinforcement learning with multi-agent support.
 * 
 * Copyright (c) 2009 Matthijs Snel
 * 
 * This program is free software: you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation, either version 3 of the License, or
 * (at your option) any later version.
 *
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program.  If not, see <http://www.gnu.org/licenses/>.
 */
package net.javlov.world.phys2d;

import java.awt.Shape;
import java.awt.geom.Rectangle2D;
import java.util.ArrayList;
import java.util.Collection;
import java.util.HashMap;
import java.util.HashSet;
import java.util.List;
import java.util.Map;
import java.util.Set;

import net.javlov.world.Body;
import net.javlov.world.grid.*;
import net.phys2d.math.ROVector2f;
//import net.phys2d.raw.Body;
import net.phys2d.raw.strategies.BruteCollisionStrategy;


public class Phys2DGridWorld extends Phys2DWorld implements IGridWorld {
	
	protected Grid grid;
	//map of each *moving* body to cells it currently intersects
	protected Map<Body, Set<GridCell>> currentCells;
	
	public Phys2DGridWorld(int iterations, float timestep, int width, int height, double cellWidth, double cellHeight ) {
		super(iterations, new BruteCollisionStrategy(), timestep);
		grid = new Grid(width, height, cellWidth, cellHeight);
		currentCells = new HashMap<Body, Set<GridCell>>(15); 
	}
	
	@Override
	public boolean addBody(Body b) {
		if ( super.addBody(b) ) {
			addToCells((Phys2DBody)b);
			return true;
		}
		return false;
	}
	
	@Override
	public boolean removeBody(Body b) {
		if ( super.removeBody(b) ) {
			removeFromCells((Phys2DBody)b);
			return true;
		}
		return false;
	}
	
	/*@Override
	public void init() {
		super.init();
		grid.clear();
		currentCells.clear();
		for ( Body b : bodies )
			addToCells(b);
	}*/
	
	public Grid getGrid() {
		return grid;
	}
	
	/**
	 * @inheritDoc
	 */
	@Override
	public List<Body> getIntersectingObjects(Shape s) {
		ArrayList<Body> objects = new ArrayList<Body>();
		Rectangle2D bounds = s.getBounds2D();
		Set<Phys2DBody> occupiers;
		if ( bounds.getWidth() > grid.getCellWidth() || bounds.getHeight() > grid.getCellHeight() )
			occupiers = getOccupiers( getIntersectingCellsLarge(s) );
		else
			occupiers = getOccupiers( getIntersectingCells(s) );
		
		for ( Body b : occupiers ) {
			if ( s.intersects(b.getFigure().getBounds2D()) )
				objects.add((net.javlov.world.Body)b);
		}
		return objects;
	}
	
	/**
	 * @inheritDoc
	 */
	@Override
	public boolean intersectsObject(Shape s) {
		for ( Body b : getOccupiers( getIntersectingCells(s) ) )
			if ( s.intersects(b.getFigure().getBounds2D()) )
				return true;
		return false;
	}
	
	protected List<GridCell> getIntersectingCellsLarge(Shape s) {
		Rectangle2D bounds = s.getBounds2D();
		List<GridCell> intersectingCells = new ArrayList<GridCell>();
		
		double 	minx = Math.max(bounds.getMinX(), 0),
				maxx = Math.min(bounds.getMaxX(), grid.getWidth()*grid.getCellWidth()),
				miny = Math.max(bounds.getMinY(), 0),
				maxy = Math.min(bounds.getMaxY(), grid.getHeight()*grid.getCellHeight());
		
		for ( double x = minx; x < maxx; x += grid.getCellWidth() )
			for ( double y = miny; y < maxy; y += grid.getCellHeight() )
				intersectingCells.add( grid.getCell(x, y) );
		
		return intersectingCells;
	}
	
	protected List<GridCell> getIntersectingCells(Shape s) {
		//IMPORTANT: the following assumes the shape is SMALLER
		//than the width & height of a cell!
		Rectangle2D bounds = s.getBounds2D();
		double cx = bounds.getCenterX(),
				cy = bounds.getCenterY();
		List<GridCell> intersectingCells = new ArrayList<GridCell>(5);
		
		GridCell cell = grid.getCell(cx, cy);
		intersectingCells.add(cell);
		//if the shape is not completely contained within the current cell, check for other
		//intersecting cells
		if ( !cell.contains(bounds) ) {
			GridCell[] neighbours = cell.getQuadrantNeightbours(cx, cy);
			for ( int j = 0; j < neighbours.length; j++ )
				if ( neighbours[j].intersects(bounds) )
					intersectingCells.add(neighbours[j]);
		}
		return intersectingCells;
	}
	
	protected Set<Phys2DBody> getOccupiers(Collection<? extends GridCell> cells) {
		Set<Phys2DBody> occupiers = new HashSet<Phys2DBody>( (int) Math.ceil(cells.size() / 0.75) );
		for ( GridCell c : cells )
			for ( Body b : c.getOccupiers() )
				occupiers.add((Phys2DBody) b);
		return occupiers;
	}
	
	public void resolve(List<? extends net.phys2d.raw.Body> bodies, float dt) {
		Phys2DBody p2db;
		List<GridCell> intersectingCells;
		/*for ( int i = bodies.size()-1; i >= 0; i-- ) {
			b = (Phys2DBody) bodies.get(i);
			System.out.println(b);
			if ( !b.isStatic() ) {
				intersectingCells = getIntersectingCells(b.getShape());
				updateCells(intersectingCells, b);
			}
		}*/
		for ( Body b : currentCells.keySet() ) {
			intersectingCells = getIntersectingCells(b.getFigure());
			updateCells(intersectingCells, b);
		}
		resolveFromCells(dt);
	}
	
	protected void updateCells(Collection<? extends GridCell> intersectingCells, Body b) {
		//IMPORTANT: the following assumes the shape of dynamic bodies is SMALLER
		//than the width & height of a cell!
		Set<GridCell> cells = currentCells.get(b);
		//nothing changed
		if ( cells.size() == intersectingCells.size() )
			return;
		
		List<GridCell> buffer = new ArrayList<GridCell>();
		//intersecting is a subset of last cells: need to remove some cells
		if ( cells.size() > intersectingCells.size() ) {
			for ( GridCell cell : cells )
				if ( !intersectingCells.contains(cell) ) {
					cell.removeBody((net.javlov.world.Body) b);
					buffer.add(cell);
				}
			cells.removeAll(buffer);
		}
		//last cells is a subset of intersecting: need to add some cells
		else {
			for ( GridCell cell : intersectingCells )
				if ( !cells.contains(cell) ) {
					cell.addBody((net.javlov.world.Body) b);
					buffer.add(cell);
				}
			cells.addAll(buffer);
		}
	}
	
	protected void resolveFromCells(float dt) {
		List<Phys2DBody> toResolve = new ArrayList<Phys2DBody>();
		for ( Set<GridCell> cellSet : currentCells.values() ) {
			for ( Phys2DBody b : getOccupiers(cellSet) )
				toResolve.add(b);
			super.resolve(toResolve, dt);
			toResolve.clear();
		}
	}
	
	protected void addToCells(Phys2DBody b) {
    	Rectangle2D.Float bounds = b.getShape().getBounds2D();
    	if ( bounds.width > grid.getCellWidth() || bounds.height > grid.getCellHeight() ) {
    		if ( !b.isStatic() )
    			throw new IllegalArgumentException(getClass() + " will behave incorrectly " +
    					"for dynamic bodies with a size larger than the cell size");
    		GridCell[][] cells = grid.getCells();
    		for(int h = 0; h < cells[0].length; h++)
        		for(int w = 0; w < cells.length; w++)
        			if ( cells[w][h].intersects(bounds) )
        				cells[w][h].addBody(b);
    	}
    	else {
    		ROVector2f pos = b.getPosition();
    		GridCell cell = grid.getCell(pos.getX(), pos.getY());
    		cell.addBody(b);
    		if ( !b.isStatic() ) {
    			Set<GridCell> cells = new HashSet<GridCell>(6);
    			cells.add(cell);
    			currentCells.put(b, cells);
    		}
    		if ( !cell.contains(bounds) ) {
    			GridCell[] neighbours = cell.getQuadrantNeightbours(pos.getX(), pos.getY());
    			for ( int i = 0; i < neighbours.length; i++ )
    				if ( neighbours[i].intersects(bounds) ) {
    					neighbours[i].addBody(b);
    					if ( !b.isStatic() )
    						currentCells.get(b).add(neighbours[i]);
    				}
    		}    		
    	}
    }
	
	protected void removeFromCells(Phys2DBody b) {
    	Rectangle2D.Float bounds = b.getShape().getBounds2D();
    	if ( bounds.width > grid.getCellWidth() || bounds.height > grid.getCellHeight() ) {
    		GridCell[][] cells = grid.getCells();
    		for(int h = 0; h < cells[0].length; h++)
        		for(int w = 0; w < cells.length; w++)
        			if ( cells[w][h].intersects(bounds) )
        				cells[w][h].removeBody((net.javlov.world.Body)b);
    	}
    	else {
    		ROVector2f pos = b.getPosition();
    		GridCell cell = grid.getCell(pos.getX(), pos.getY());
    		cell.removeBody((net.javlov.world.Body)b);
    		if ( !b.isStatic() )
    			currentCells.remove(b);
    		if ( !cell.contains(bounds) ) {
    			GridCell[] neighbours = cell.getQuadrantNeightbours(pos.getX(), pos.getY());
    			for ( int i = 0; i < neighbours.length; i++ )
    				if ( neighbours[i].intersects(bounds) )
    					neighbours[i].removeBody((net.javlov.world.Body)b);
    		}    		
    	}
    }
	
	
	
	
	/*
	public void resolve(List<Body> bodies, float dt) {
		Body b;
		ROVector2f pos;
		GridCell cell;
		Rectangle2D.Float bounds;
		Set<GridCell> cells;
		List<GridCell> tempCells = new ArrayList<GridCell>(); //temp list to store cells to remove
		for ( int i = bodies.size()-1; i >= 0; i-- ) {
			b = bodies.get(i);
			if ( b.isStatic() ) {
				//System.out.println("Breaking at " + i);
				break;
			}
			bounds = b.getShape().getBounds2D();
			pos = b.getPosition();
			//the cell that currently contains the centre of the body
			cell = grid.getCell(pos.getX(), pos.getY());
			//double check that the cell contains the body, there could've been a reset
			if ( !cell.hasBody(b) )
				cell.addBody(b);
			
			//the list of cells that the body previously intersected
			cells = currentCells.get(b);
			cells.add(cell);
			
			//if the body is completely contained within the current cell
			if ( cell.contains(bounds) ) {
				if ( cells.size() > 1 ) {
					for ( GridCell c : cells ) {
						if ( !c.equals(cell) ) {
							c.removeBody(b);
							tempCells.add(c);
						}
					}
					cells.removeAll(tempCells);
				}
			}
			//if the body is also intersecting some other cells
			else {
				tempCells.add(cell);
				//get the cells neighbouring the cell quadrant in which the body is (3 cells)
				GridCell[] neighbours = cell.getQuadrantNeightbours(pos.getX(), pos.getY());
				for ( int j = 0; j < neighbours.length; j++ ) {
					if ( neighbours[j].intersects(bounds) ) {
						cells.add(neighbours[j]);
						tempCells.add(neighbours[j]);
						if ( !neighbours[j].hasBody(b) )
							neighbours[j].addBody(b);
					}
				}
				//tempCells contains the cells that are intersecting the body
				for ( GridCell c : cells ) {
					if ( !tempCells.contains(c) )
						c.removeBody(b);
				}
				cells.retainAll(tempCells);
			} 
			tempCells.clear();
		}
		//now currentCells contains the cells (usually just 1) that each body is currently 
		//intersecting
		resolveFromCells(dt);
	}*/
}
